#include <iostream>
#include <unistd.h>
#include "Algorithm/AlgorithmUR.h"
#include "URDriver/Xbox.h"
#include "SharedTest.cpp"

using namespace std;
using namespace zanxin;

int main(int argc, char* argv[])
{
    std::string ip{""};
    if(argc < 2){
//        cout<<"请输入需要连接的机械臂ip地址"<<endl;
//        return 0;
        ip = "192.168.71.133";
    }else{
        ip = argv[1];
    }
    cout<<"正在连接的机械臂ip地址: "<<ip<<endl;
    AlgorithmUR a{ip};
//    Xbox xboxDev{};
    int xbox_fd = 0;
    xbox_map_t map;
    int len = 0;
    memset(&map, 0, sizeof(xbox_map_t));

//    xbox_fd = xboxDev.Open("/dev/input/js0");
    if(xbox_fd < 0)
    {
        return -1;
    }
    // 共享内存
    SharedData shmbuf;
    double prePos[6] = {0.0};
//    auto timer = std::chrono::high_resolution_clock::now();
    while(1)
    {
//        len = xboxDev.MapRead(&map);
//        if (len < 0)
//        {
//            usleep(10*1000);
//            continue;
//        }
        shmbuf = getdata();
//        a.MoveTool(map);
        // test cycle time
//        if(shmbuf.type == 5){
//            shmbuf.type = 0;
//            setdata(shmbuf);
//            cout<<"timecycle: "<<std::chrono::duration_cast<std::chrono::microseconds>
//                  (std::chrono::high_resolution_clock::now() - timer).count()<<endl;
//        }
//        timer = std::chrono::high_resolution_clock::now();
//        continue;
        bool skip = false;
        double diff[6] = {0.0};// 增量值
        for(unsigned long i = 0; i < 3; ++i){
            diff[i] = (shmbuf.data[i] - prePos[i]);
            prePos[i] = shmbuf.data[i];// 更新上一次值
            if(fabs(diff[i]) > 1.0){
                skip = true;
            }
        }
        for(unsigned long i = 3; i < 6; ++i){
            diff[i] = (shmbuf.data[i] - prePos[i]);
            prePos[i] = shmbuf.data[i];// 更新上一次值
            if(fabs(diff[i]) > 5.0){
                skip = true;
            }
        }
        if(!skip){
            a.MoveTool(diff);
        }else{
            printf("xyz: %f %f %f\n",
                   diff[0], diff[1], diff[2]);

        }
        usleep(8000);
//        printf("\rTime:%8d A:%d B:%d X:%d Y:%d LB:%d RB:%d start:%d back:%d home:%d LO:%d RO:%d XX:%-6d YY:%-6d LX:%f LY:%f RX:%-6d RY:%-6d LT:%-6d RT:%-6d\n",
//                map.time, map.a, map.b, map.x, map.y, map.lb, map.rb, map.start, map.back, map.home, map.lo, map.ro,
//                map.xx, map.yy, map.lx, map.ly, map.rx, map.ry, map.lt, map.rt);
    }
    return 0;
}
